#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include "opencv2/objdetect/objdetect.hpp"
#include "opencv2/highgui/highgui_c.h"

#include <iostream>
#include <stdio.h>

namespace enc = sensor_msgs::image_encodings;

using namespace cv;
using namespace std;

static const char WINDOW[] = "Image window";
static const char WINDOW2[] = "Image window2";
static const char entrada1[] = "usb_cam_l/image_raw";
static const char entrada2[] = "usb_cam_r/image_raw";
static const char face_cascade_name[] = "/home/usuario_robot/Pablo/sandbox/pruebaOp/src/haarcascade_frontalface_alt.xml";
static const char eyes_cascade_name[] = "/home/usuario_robot/Pablo/sandbox/pruebaOp/src/haarcascade_eye_tree_eyeglasses.xml";
static const char window_name[] = "Capture - Face detection";

class ImageConverter
{
  ros::NodeHandle nh_;
  image_transport::ImageTransport it_;
  image_transport::Subscriber image_sub_;
  image_transport::Publisher image_pub_;
  int j;
	
	CascadeClassifier face_cascade;
	CascadeClassifier eyes_cascade;


  
public:
  ImageConverter(int i)
    : it_(nh_)
  {
  if( !face_cascade.load( face_cascade_name ) ){ printf("--(!)Error loading\n"); };
  if( !eyes_cascade.load( eyes_cascade_name ) ){ printf("--(!)Error loading\n"); };
    j=i;
    //image_pub_ = it_.advertise("out", 1);
if (i)
    image_sub_ = it_.subscribe(entrada1, 1, &ImageConverter::imageCb, this);
else
    image_sub_ = it_.subscribe(entrada2, 1, &ImageConverter::imageCb, this);
if (i)
    namedWindow(WINDOW);
else
    namedWindow(WINDOW2);
  }

  ~ImageConverter()
  {
if (j)
    destroyWindow(WINDOW);
else
    destroyWindow(WINDOW2);
    
  }

Mat detectAndDisplay( Mat frame )
	{
	   std::vector<Rect> faces;
	   Mat frame_gray;

	   cvtColor( frame, frame_gray, COLOR_BGR2GRAY );
	   equalizeHist( frame_gray, frame_gray );
	   //-- Detect faces
	  face_cascade.detectMultiScale( frame_gray, faces, 1.1, 2, 0|CASCADE_SCALE_IMAGE, Size(30, 30) );

	   for( size_t i = 0; i < faces.size(); i++ )
	    {
	     Point center( faces[i].x + faces[i].width/2, faces[i].y + faces[i].height/2 );
	      ellipse( frame, center, Size( faces[i].width/2, faces[i].height/2), 0, 0, 360, Scalar( 255, 0, 255 ), 2, 8, 0 );

	      Mat faceROI = frame_gray( faces[i] );
	      std::vector<Rect> eyes;

	      //-- In each face, detect eyes
	      eyes_cascade.detectMultiScale( faceROI, eyes, 1.1, 2, 0 |CASCADE_SCALE_IMAGE, Size(30, 30) );

	      for( size_t j = 0; j < eyes.size(); j++ )
	       {
		 Point eye_center( faces[i].x + eyes[j].x + eyes[j].width/2, faces[i].y + eyes[j].y + eyes[j].height/2 );
		 int radius = cvRound( (eyes[j].width + eyes[j].height)*0.25 );
		 circle( frame, eye_center, radius, Scalar( 255, 0, 0 ), 3, 8, 0 );
	       }
	    }
	   //-- Show what you got
	   return frame;
	}




  void imageCb(const sensor_msgs::ImageConstPtr& msg)
  {
    Mat display;
    cv_bridge::CvImagePtr cv_ptr;
    try
    {
      cv_ptr = cv_bridge::toCvCopy(msg, enc::BGR8);
    }
    catch (cv_bridge::Exception& e)
    {
      ROS_ERROR("cv_bridge exception: %s", e.what());
      return;
    }

    display =detectAndDisplay(cv_ptr->image);


    if(j)
    imshow(WINDOW, display);
    else
    imshow(WINDOW2, display);
    waitKey(3);
    
    //image_pub_.publish(cv_ptr->toImageMsg());
  }

};

int main(int argc, char** argv)
{
  ros::init(argc, argv, "image_converter");
  ImageConverter ic1(1);
  ImageConverter ic2(0);
  ros::spin();
  return 0;
}
